#include <iostream>
#include <vector>
#include <type_traits>
#include <array>

// 1. 定义不同类型的点
struct Point2D
{
    float x, y;
};

struct Point3D
{
    float x, y, z;
};

struct PointXYZRGB
{
    float x, y, z;
    uint8_t r, g, b;
};

// 2. 定义点类型的traits
template <typename T>
struct PointTraits
{
    // 默认实现，可以被特化覆盖
    static constexpr bool is_point = false;
    static constexpr int dimensions = 0;
    static constexpr bool has_color = false;
};

// Point2D的特化
template <>
struct PointTraits<Point2D>
{
    static constexpr bool is_point = true;
    static constexpr int dimensions = 2;
    static constexpr bool has_color = false;

    // 获取点的坐标数组
    static std::array<float, 2> getCoordinates(const Point2D &p)
    {
        return {p.x, p.y};
    }

    // 设置点的坐标
    static void setCoordinates(Point2D &p, const std::array<float, 2> &coords)
    {
        p.x = coords[0];
        p.y = coords[1];
    }
};

// Point3D的特化
template <>
struct PointTraits<Point3D>
{
    static constexpr bool is_point = true;
    static constexpr int dimensions = 3;
    static constexpr bool has_color = false;

    static std::array<float, 3> getCoordinates(const Point3D &p)
    {
        return {p.x, p.y, p.z};
    }

    static void setCoordinates(Point3D &p, const std::array<float, 3> &coords)
    {
        p.x = coords[0];
        p.y = coords[1];
        p.z = coords[2];
    }
};

// PointXYZRGB的特化
template <>
struct PointTraits<PointXYZRGB>
{
    static constexpr bool is_point = true;
    static constexpr int dimensions = 3;
    static constexpr bool has_color = true;

    static std::array<float, 3> getCoordinates(const PointXYZRGB &p)
    {
        return {p.x, p.y, p.z};
    }

    static void setCoordinates(PointXYZRGB &p, const std::array<float, 3> &coords)
    {
        p.x = coords[0];
        p.y = coords[1];
        p.z = coords[2];
    }

    static std::array<uint8_t, 3> getColor(const PointXYZRGB &p)
    {
        return {p.r, p.g, p.b};
    }

    static void setColor(PointXYZRGB &p, const std::array<uint8_t, 3> &color)
    {
        p.r = color[0];
        p.g = color[1];
        p.b = color[2];
    }
};

// 3. 使用std::enable_if实现类型特化的算法

// 计算点到原点的距离
template <typename PointT>
typename std::enable_if<PointTraits<PointT>::is_point, float>::type
distance_to_origin(const PointT &point)
{
    auto coords = PointTraits<PointT>::getCoordinates(point);
    float sum_squared = 0.0f;

    for (float coord : coords)
    {
        sum_squared += coord * coord;
    }

    return std::sqrt(sum_squared);
}

// 仅适用于3D点的法线计算
template <typename PointT>
typename std::enable_if<
    PointTraits<PointT>::is_point &&
        PointTraits<PointT>::dimensions == 3,
    std::array<float, 3>>::type
compute_normal(const std::vector<PointT> &points, size_t index)
{
    // 简化的法线计算，实际应用中会更复杂
    if (index >= points.size() || points.size() < 3)
    {
        return {0, 0, 1}; // 默认向上的法线
    }

    // 获取相邻点
    size_t prev = (index > 0) ? index - 1 : points.size() - 1;
    size_t next = (index < points.size() - 1) ? index + 1 : 0;

    auto p0 = PointTraits<PointT>::getCoordinates(points[prev]);
    auto p1 = PointTraits<PointT>::getCoordinates(points[index]);
    auto p2 = PointTraits<PointT>::getCoordinates(points[next]);

    // 计算两个向量
    std::array<float, 3> v1 = {p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]};
    std::array<float, 3> v2 = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};

    // 叉积计算法线
    std::array<float, 3> normal = {
        v1[1] * v2[2] - v1[2] * v2[1],
        v1[2] * v2[0] - v1[0] * v2[2],
        v1[0] * v2[1] - v1[1] * v2[0]};

    // 归一化
    float length = std::sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]);
    if (length > 0)
    {
        normal[0] /= length;
        normal[1] /= length;
        normal[2] /= length;
    }

    return normal;
}

// 仅适用于带颜色点的颜色处理
template <typename PointT>
typename std::enable_if<
    PointTraits<PointT>::is_point &&
        PointTraits<PointT>::has_color,
    void>::type
adjust_brightness(PointT &point, float factor)
{
    auto color = PointTraits<PointT>::getColor(point);
    std::array<uint8_t, 3> new_color;

    for (size_t i = 0; i < 3; ++i)
    {
        float adjusted = color[i] * factor;
        new_color[i] = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, adjusted)));
    }

    PointTraits<PointT>::setColor(point, new_color);
}

// 4. 使用C++17的if constexpr简化条件编译
template <typename PointT>
void process_point_cloud(std::vector<PointT> &points)
{
    // 静态断言确保我们处理的是点类型
    static_assert(PointTraits<PointT>::is_point, "PointT must be a point type");

    std::cout << "Processing " << points.size() << " points with "
              << PointTraits<PointT>::dimensions << " dimensions" << std::endl;

    // 对每个点进行处理
    for (size_t i = 0; i < points.size(); ++i)
    {
        // 计算到原点的距离
        float dist = distance_to_origin(points[i]);

        // 使用if constexpr进行编译时条件分支
        if constexpr (PointTraits<PointT>::dimensions == 3)
        {
            // 只为3D点计算法线
            auto normal = compute_normal(points, i);
            std::cout << "Point " << i << " normal: "
                      << normal[0] << ", " << normal[1] << ", " << normal[2] << std::endl;
        }

        if constexpr (PointTraits<PointT>::has_color)
        {
            // 只为带颜色的点调整亮度
            // 根据距离调整亮度：越远越暗
            float brightness_factor = 1.0f / (1.0f + dist * 0.1f);
            adjust_brightness(points[i], brightness_factor);
        }
    }
}

// 5. 主函数示例
int main()
{
    // 创建不同类型的点云
    std::vector<Point2D> points2d = {{1, 2}, {3, 4}, {5, 6}};
    std::vector<Point3D> points3d = {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};
    std::vector<PointXYZRGB> pointsRgb = {
        {1, 2, 3, 255, 0, 0},
        {4, 5, 6, 0, 255, 0},
        {7, 8, 9, 0, 0, 255}};

    // 处理不同类型的点云
    std::cout << "\nProcessing 2D points:" << std::endl;
    process_point_cloud(points2d);

    std::cout << "\nProcessing 3D points:" << std::endl;
    process_point_cloud(points3d);

    std::cout << "\nProcessing RGB points:" << std::endl;
    process_point_cloud(pointsRgb);

    // 展示颜色调整结果
    std::cout << "\nAdjusted RGB values:" << std::endl;
    for (size_t i = 0; i < pointsRgb.size(); ++i)
    {
        auto color = PointTraits<PointXYZRGB>::getColor(pointsRgb[i]);
        std::cout << "Point " << i << " color: "
                  << static_cast<int>(color[0]) << ", "
                  << static_cast<int>(color[1]) << ", "
                  << static_cast<int>(color[2]) << std::endl;
    }

    return 0;
}